AD-E402  596 


Special  Publication  ARFSD-SP-94001 


BRIEF  TUTORIAL  ON  THE  KALMAN  FILTER 


John  Podesta 


94-36039 


mil  nr 


3 


i  iC 

ELECTED 

W23  1994.1'  ' 

$St"B 


DTIC  QUALITY  DTEPECTED  3 


November  1994 


US  ARMY 

TAM<  AUTOMOTIVE  AM) 
ARMAMENTS  COMMAND 


ARMAMENT  ROE  CENTER 


U.S.  ARMY  ARMAMENT  RESEARCH,  DEVELOPMENT  AND 

ENGINEERING  CENTER 

Fire  Support  Armaments  Directorate 

Picatinny  Arsenal,  New  Jersey 


Approved  for  public  release;  distribution  is  unlimited. 


The  views,  opinions,  and/or  findings  contained  in  this 
report  are  those  of  the  authors(s)  and  should  not  be 
construed  as  an  official  Department  of  the  Army  posi¬ 
tion.  policy,  or  decision,  unless  so  designated  by  other 
documentation. 

The  citation  in  this  report  of  the  names  of  commercial 
firms  or  commercially  available  products  or  services  does 
not  constitute  official  endorsement  by  or  approval  of 
the  U.S.  Government. 

Destroy  this  report  when  no  longer  needed  by  any  method 
that  will  prevent  disclosure  of  its  contents  or  recon¬ 
struction  of  the  document.  Do  not  return  to  the  origi¬ 
nator. 


REPOPT  DOCUMENTATION  PAGE 

Fonti  Approved  OMB  No.  07040188 

Public  reporting  burden  ter  this  colection  of  information  is  aettmatsd  to  average  1  hour  per  response,  includteg  the  time  tor  reviewing 
instructions,  searching  existing  data  sources,  gathering  and  maintaining  the  data  needed,  and  completing  and  reviewing  the  collection  of 
information.  Send  comments  regarding  this  burden  estimate  or  any  other  aspect  of  this  cotodion  of  information,  including  suggestions  for 
reducing  this  burden,  to  Washington  Headquarters  Services.  Directorate  for  Information  Operation  and  Reports,  1215  Jefferson  Davis 
Highway,  Suite  1204,  Arlington.  VA  22202-4302.  and  to  the  Office  of  Management  and  Budget.  Paperwork  Reduction  Project  (0704- 
0188).  Washington.  DC  20503. 

1.  AGENCY  USE  ONLY  (Learn  blank) 

2.  REPORT  DATE 

November  1994 

a  REPORT  TYPE  AND  DATES  COVERED 
AnnuaVSep  91  to  Nov  91 

4.  TITLE  AND  SUBTITLE 

BRIEF  TUTORIAL  ON  THE  KALMAN  FILTER 

5.  FUNDING  NUMBERS 

6.  AUTHOR(S) 

John  Podesta 

7.  PERFORMING  ORGANIZATION  NAME(S)  AND  ADDRESSES(S) 

ARDEC,  FSAC 

Precision  Munitions  Division  (SMCAR-FSP-E) 

Picatinny  Arsenal,  NJ  07806*5000 

8.  PERFORMING  ORGANIZATION 
REPORT  NUMBER 

Special  Publication 
ARFSD-SP-94001 

9. SPONSORING/MONITORING  AGENCY  NAME(S)  AND  ADDRESS(S) 

ARDEC.  IMD 

Information  Research  Center  (AMSTA-AR-IMC) 

Picatinny  Arsenal,  NJ  07806-5000 

10.  SPONSORING/MONITORING 

AGENCY  REPORT  NUMBER 

11.  SUPPLEMENTARY  NOTES 

12a.  DISTRIBUTION/A VAI LABILITY  STATEMENT 

Approved  for  public  release;  distribution  is  unlimited. 

12b.  DISTRIBUTION  CODE 

13.  ABSTRACT  (Maximum  200  words) 

A  derivation  of  the  Kalman  filter  equations  is  presented  which  should  provde  a  concise  introduction  to  Kalman  filter 
theory  for  scientists,  engineers,  and  mathematicians  alike.  An  elementary  derivation  of  the  basic  Kalman  filter,  the  1- 
step  Kalman  predictor,  is  given  first  in  1 -dimension  and  then  in  n-dimension.  The  well  known  prediction-correction 
formulation  of  the  Kalman  filter  equations  are  derived  for  the  filtered  estimate,  or  current  state  estimate.  Then,  the 
state  prediction  is  obtianed  from  the  filtered  estimate.  It  is  assumed  that  the  reader  has  a  background  in  probability 
theory  and  some  exposure  to  stochastic  processes. 

14.  SUBJECT  TERMS 

Kalman  filter  Kalman  filtering  Linear  filtering  Estimation  theory 

Recursive  estimation  Least  squares  estimation  Linear  prediction  theory 

Stochastic  processes  Stochastic  control 

15.  NUMBER  OF  PAGES 

31 

16.  PRICE  CODE 

17.  SECURITY  CLASSIFICATION 
OF REPORT 

UNCLASSIFIED 

18.  SECURITY  CLASSIFICATION 
OF  THIS  PAGE 

UNCLASSIFIED 

19.  SECURITY  CLASSIFICATION 
OF  ABSTRACT 
UNCLASSIFIED 

20.  LIMITATION  OF  ABSTRACT 

SAR 

NSN  7540-01  280-5500  Standard  Form  298  (Rev.  2-89) 

Prescribed  by  ANSI  Std.  Z39-18 
286-102 


Contents 


1  Introduction  1 

2  Least  Squares  Estimation  1 

3  Prediction  Problem  4 

4  Kalman  Filter  in  1-Dimension  7 

5  Kalman  Filter  in  n-Dimensions  10 

6  Standard  Kalman  Filter  Equations  14 

7  Initial  Conditions  for  the  Standard  Kalman  Filter  17 

8  Prediction-Correction  Formulation  19 

9  Summary  21 

References  22 

Appendix  25 

Distribution  List  31 


Accession  For 


MTIS  GRAftI 
DTIC  TAB 
Unannounced 
Justification. 


IF4- 


j  Distribution/^*^ 

Availability  Qedcs 


By. 


rvail  and/or* 
Special 


□  □ 


1  Introduction 


Since  its  invention  by  Rudolf  E.  Kalman  in  1960,  (refs.  1  through  5)[l]-[5]  the  Kalman 
filter  has  found  widespread  applications  in  commercial  industry,  the  defense  industry,  and 
in  academia.  (In  this  report,  all  references  are  enclosed  in  square  brackets,  i.e.,  [1]  stands 
for  reference  1,  etc.;  and  all  equations  are  indicated  by  equation  numbers  in  parentheses, 
i.e.,  (1)  stands  for  equation  1,  etc.)  The  Kalman  filter  is  especially  important  in  military 
applications  where  guidance,  control,  and  signal  processing  systems  must  achieve  a  high  level 
of  performance.  Although  many  books  and  articles  have  been  published  on  the  theory  of  the 
Kalman  filter  [6]— [11]  the  derivation  of  the  Kalman  filter  equations  is  often  difficult  to  follow. 
Unfortunately,  this  may  leave  the  unacquainted  engineer  or  scientist  without  quick  access 
to  the  main  results.  For  many,  there  is  little  time  to  wade  through  all  the  mathematical 
proofs  and  derivations  which  lead,  finally,  to  the  Kalman  filter  equations.  Nevertheless, 
the  derivation  of  the  filter  equations  is  not  difficult,  and  for  educational  purposes  it  is 
desireable  to  simplify  it  as  much  as  possible.  The  purpose  of  this  brief  report  is  to  present 
a  straightforward  derivation  of  the  Kalman  filter  which  is  concise  and  simple.  A  simple 
derivation  of  the  filter  equations  should  make  the  theory  readily  accessible  to  anyone  who 
is  interested. 

It  is  assumed  that  the  reader  has  achieved  a  certain  level  of  mathematical  maturity 
which  includes  a  good  knowledge  of  calculus,  linear  algebra,  and  probability  theory.  Also, 
the  reader  should  be  familiar  with  linear  systems  theory  and  the  state  space  description 
of  linear  systems.  Furthermore,  it  is  desireable,  though  not  essential,  that  the  reader  has 
had  an  introduction  to  stochastic  processes.  For  those  who  are  not  familiar  with  stochastic 
processes,  section  3  may  be  safely  omitted  without  loss  of  continuity.  Throughout  this 
report  all  vectors  and  matrices  will  be  written  in  boldface  type,  and  it  is  assumed  that  all 
vectors  and  matrices  are  real  valued.  Regarding  terminology,  the  terms  random  variable 
and  random  vector  will  be  used  interchangeably  since  this  should  not  lead  to  confusion. 

The  report  is  structured  as  follows.  The  method  of  least  squares  is  reviewed  in  section 
number  2.  In  section  3  it  is  shown  that  for  statistical  reasons  the  least  squares  estimate  of  a 
random  quantity  must  be  considered  to  be  a  random  variable.  The  Kalman  filter  equations 
are  derived  in  sections  4,  5,  6,  and  7.  In  section  4,  the  basic  Kalman  filter,  or  Kalman 
predictor,  is  derived  in  1-dimension.  This  is  then  extended  to  the  n— dimensional  case  in 
section  5.  In  section  6,  the  standard  Kalman  filter  equations  are  derived  in  the  general 
n— dimensional  case.  The  initial  conditions  for  the  standard  Kalman  filter  require  some 
special  consideration,  and  these  are  derived  in  section  7.  Relations  between  the  Kalman 
predictor  and  the  standard  Kalman  filter  are  developed  in  section  8. 

2  Least  Squares  Estimation 

Consider  a  physical  system  in  a  steady  state  (not  time  varying)  and  let  the  state  be 
described  by  a  state  variable  x.  In  the  steady  state,  x  is  a  constant.  Suppose  that  a 
measurement  made  on  the  system  yields  a  value  z.  The  measurement  process  is  not  perfect 
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and  will  inevitably  contain  errors,  i.e.,  experimental  errors.  Thus  the  measurement  process 
can  be  modeled  by  the  measurement  equation 

2  =  Hi  +  t7,  (1) 

where  if  is  a  scale  factor  which  is  built  into  the  measurement  equipment  and  v  is  a  random 
variable  which  characterizes  the  measurement  errors.  The  probability  density  function 
(PDF)  of  the  random  variable  v  is  assumed  given.  Typically,  v  has  a  normal  distribution 
with  zero  mean  and  known  variance.  Once  a  measurement  2  has  been  obtained  in  the  lab, 
the  problem  is  to  find  an  estimate  £  of  the  state  x  based  on  the  noise-corrupted  measurement 
2.  It  is  intuitively  obvious  that  if  the  error  is  small  then  the  desired  estimate  is  simply  2/ H. 
In  mathematical  terms,  this  problem  can  be  solved  using  the  method  of  least  squares.  Let 
£  be  an  unknown  variable  and  define  the  error 

e  =  2  -  Hi.  (2) 


Since  2  and  H  are  fixed  real  numbers,  the  error  is  a  function  of  the  variable  £.  In  the 
method  of  least  squares,  the  best  estimate  £  is  that  which  minimizes  the  square  error 


e  =  (2  —  Hi)2. 

(3) 

Since  e  =  e(x)  is  a  function  of  £,  this  will  be  a  minimum  if 

ds 

(4) 

d£=°- 

Differentiating  (3)  yields 

%  =  2(*  -  =  0. 

(5) 

Hence,  the  solution  is 

.  2 

X=H' 

(6) 

as  was  expected.  To  verify  that  this  is  a  minimum  and  not  a  maximum  examine  the  second 
derivative 


Hi  =  >  »■  P) 

Since  this  is  positive,  the  value  of  £  given  by  (6)  is  indeed  a  minimum. 

Next,  consider  the  same  problem  in  n— dimensions.  The  generalized  measurement  model 
is  given  by 

z  =  Hx  +  v,  (8) 

where  x  is  an  n— dimensional  column  vector,  z  is  an  m-dimensional  column  vector  of 
measurements,  and  H  is  an  m  x  n  matrix.  The  m— dimensional  column  vector  v  is  a  zero 
mean  random  vector  that  describes  the  measurement  errors.  The  PDF  of  v  is  assumed 
given.  For  simplicity,  it  will  be  assumed  that  m  =  n,  and  that  H  is  nonsingular.  Observe 
that  the  quantities  z  and  H  are  given  and  the  state  vector  x  is  to  be  estimated  from  the 
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measurements.  Thus,  the  problem  is  to  find  the  estimate  x  of  the  state  vector  x  which 
minimizes  the  square  error 

e  =  II*  -  HS||J.  (9) 

The  square  error  is  a  scalar  and  may  be  written 

e  =  (z  -  H*)t(z  -  Hit),  (10) 


where  the  superscript  T  denotes  the  transpose.  Expanding  the  product  this  becomes 


e  =  zTz  -  zTHx  -  *tHtz  +  5ctHtHx. 


(11) 


All  quantities  are  known  except  for  x.  Therefore,  the  square  error  e  is  a  function  of  the 
components  of  x,  that  is, 


£  —  /(*  1,  •  •  • ,  ®n)* 

Using  a  well  known  result  from  advanced  calculus,  this  function  will  be  a  minimum  if 

!Lm0 

dii  ’ 


(12) 

(13) 


for  i  =  1,2, . .  .,n.  It  is  convenient  to  define  the  derivative  of  a  scalar  with  respect  to  a 
vector  to  be  the  column  vector  with  components 


£>|«h 

II 

'  «• 

(14) 

where 

II 

(15) 

i  =  1, 2, . . .,  n.  Thus,  the  square  error  will  be  a  minimum  if 

de 

(16) 

o 

II 

:K 

The  terms  in  equation  (11)  can  be  differentiated  by  using  the  following  identities: 


iiiyTx)  = y' 

(17) 

=  y. 

(18) 

^(xTAx)  =  2  Ax, 

(19) 

where  x  and  y  are  arbitrary  vectors  and  A  is  an  arbitrary  matrix.  These  relations  are  easy 
to  prove  by  writting  them  out  in  component  form.  Proceeding  with  the  differentiation  of 
equation  (11),  it  follows  that 


de 

dx 


-HTz  -  Hrz  +  2HtH*  =  0. 


(20) 
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Rearranging  terms,  this  becomes 


HrH*  =  H  rz. 


(21) 


This  system  of  linear  equations  are  called  the  normal  equations.  Clearly,  x  will  minimize 
the  square  error  (9)  if  it  is  a  solution  of  the  normal  equations  (21).  If  H  is  nonsingular,  i.e., 
invertible,  then  the  solution  is  given  by 


x  =  H-1z.  (22) 

This  is  the  least  squares  estimate  of  the  state  vector  x  based  on  the  noise-corrupted  vector 
of  measurements  z.  Note  the  close  similarity  with  the  1-dimensional  solution  (6).  In  the 
general  case  when  m  and  n  are  arbitrary,  the  solution  to  the  normal  equations  is  given  by 

*  =  (HTH)_1HTz,  (23) 

provided  that  the  indicated  inverse  exists. 

3  Prediction  Problem 

In  the  derivation  of  the  Kalman  filter  equations  in  section  4,  the  estimate  x  is  considered 
to  be  a  random  variable  and  not  a  numerical  vector  as  it  was  in  section  2.  The  reason  for 
this  difference  of  interpretation  will  be  explained  in  this  section.  For  simplicity,  only  scalar 
(1-dimensional)  variables  will  be  considered  here. 

The  linear  prediction  of  stochastic  processes  involves  the  following  problem.  Given  a 
realization  of  a  random  process  with  the  observed  values  xo,  Xi, . . . ,  xjv,  . . . ,  find  an  estimate 
of  the  value  x^v+i  of  the  process  at  time  N  +  1  based  on  the  preceeding  values  up  to  and 
including  xjy.  This  is  called  the  prediction  problem.  The  estimate  is  called  linear  if  it  is  a 
linear  function  of  the  variables  xo,xi, . . .  ,x/v,  that  is,  if  the  estimate  has  the  linear  form 

N 

xn+i  =  (24) 

fc=sO 

It  is  possible  to  solve  this  problem  using  the  method  of  least  squares  by  minimizing  the 
squared  error 

e  =  ||xiv+i  -  xN+i||2.  (25) 

By  inserting  the  expression  (24)  into  equation  (25),  the  coefficients  ao,ai,...,ajv  may  be 
computed  using  the  method  of  the  previous  section.  However,  the  solution  for  the  prediction 
coefficients  <*o,  «i, . . . ,  will  be  different  for  different  realizations  of  the  same  process.  That 
is  to  say  that  if  Xq,x^, . .  .,x'N  were  another  realization  of  the  same  stochastic  process,  then 
the  resulting  solution  for  the  a*  will  be  different.  It  is  desireable  to  choose  the  coefficients 
so  that  the  same  set  of  coefficients  may  be  used  for  any  arbitrary  realization  of  the  process 
x(n).  That  is,  so  that  in  a  statistical  sense  the  same  coefficients  work  equally  well  for 
any  realization  of  the  stochastic  process.  To  accomplish  this,  it  is  necessary  to  rephrase 
the  problem  as  follows:  Given  the  random  variables  Xo,xj, . . .  ,xjv,  find  a  random  variable 
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Xjv+i  which  is  a  linear  combination  of  the  random  variables  xo,xi,..  .,xjv,  such  that  the 
mean  square  error 

e  =  f?[||x*+i  -  x/v+i||2]  (26) 

is  a  minimum.  In  this  case,  the  resulting  estimate  is  a  random  variable  x^+i  which  approx¬ 
imates  the  random  variable  xjv+i  in  the  sense  that  the  variance  of  the  error  e  =  (x  —  x)  is 
a  minimum.  This  is  called  the  least  squares  estimate  or  minimum  variance  estimate  of  the 
random  variable  x/v+i- 

Problem  Statement:  Let  x(n)  be  a  weakly  stationary  stochastic  process  and  assume 
that  2?[x(n)]  =  0.  Fix  N.  The  problem  is  to  determine  the  coefficients  ai,a2, . .  .,ajv,  so 
that  the  random  variable  defined  by 


N 

y  =  £]<***#-*,  (27) 

*=1 


minimizes  the  mean  square  error 


e  =  £[||aw  -  y||2].  (28) 

This  problem  was  first  considered  by  the  Russian  mathematician  Kolmogorov  in  1938.  A 
translation  of  Kolmogorov’s  original  paper  can  be  found  in  reference  [12].  The  solution  is 
so  straightforward  that  Kolmogorov  did  not  even  bother  to  write  it  down. 

Problem  Solution:  The  mean  square  error  is  given  by 

e  =  £[x2  -  2 xNy  +  y2].  (29) 

Substituting  the  expression  (27)  for  y  this  becomes 

N  N  N 

e  —  E[x2N  -  2  akXNXN-k  +  X)  ajakxN-jXN-k]-  (30) 

fc=l  j-1  k=  1 

By  the  definition  of  the  autocorrelation  function  R(n,  m),  which  for  a  stationary  process 
takes  the  form 

E[xnxm]  =  R{n  -  m),  (31) 

the  mean  square  error  (30)  can  be  written 

N  N  N 

£  =  R( 0)  -  2  £  a*R(fc)  +  £  £  ajakR(j  -  k).  (32) 

*=i  j= l  fc=i 

Since  the  autocorrelation  function  is  known,  e  can  be  considered  to  be  a  function  of  the 
variables  a\, o2, . . . , a h,  that  is, 


£  =  f(ai,a2,  •  •  •,  G;v). 


(33) 
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Hence,  the  values  of  the  coefficients  a*  which  make  f{ax, . .  .,ajv)  a  minimum  must  satisfy 
the  equations 


de_ 

da\ 

de 

da2 


0, 

0, 


(34) 

(35) 


de 

datf 


=  0, 


Differentiating  equation  (32)  with  respect  to  a*  it  follows  that 

3  S 

P -  =  -2 R(k)  +  2  £  ajR(j  -  k)  =  0, 

°*  i=i 

where  k  =  This  yields  the  nonhomogeneous  system  of  linear  equations 

X>;*(i  -*)  =  *(*), 

i= i 

where  Jfc  =  1, . . N.  In  matrix  form,  this  may  be  written 

Ax  =  b, 


where 


A  = 


\W- 1) 


x  = 


R(l) 

m 

m 

•  •  • 

m 

m 

R(  2) 

•  •  • 

• 

ai\ 

/ 

'Rx 

i  > 

and 

b= 

• 

ajv/ 

\ 

<Rn 

R(N-  2) 

m  I 


If  the  matrix  A  is  nonsingular,  then  the  system  (39)  has  the  solution 

x  =  A-1b. 


(36) 

(37) 

(38) 

(39) 

(40) 

(41) 

(42) 


This  gives  the  solution  for  the  prediction  coefficients  Oj, . . .,  a^.  For  a  good  introduction  to 
the  linear  prediction  theory  of  stochastic  processes,  the  reader  should  consult  the  original 
edition  of  the  book  by  Yaglom  [13] 

The  solution  of  the  prediction  problem  is  not  really  important  here.  What  is  important 
is  the  idea  involved  in  the  formulation  of  the  problem.  The  most  important  point  of  this 
section  is  that  in  a  statistical  sense  the  estimate  xjv  must  be  a  random  variable,  not  a 
number.  In  practice,  if  a  particular  (numerical)  realization  •  -,xn-i  is  observed, 

then  the  estimation  formula  (27)  may  be  used  to  obtain  a  numerical  result.  However, 
the  estimation  formula  is  a  general  result  that  applies  for  any  realization  of  the  stochastic 
process. 
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4  Kalman  Filter  in  1-Dimension 


In  this  section,  the  Kalman  filter  equations  will  be  derived  for  the  1-step  Kalman  pre¬ 
dictor.  Consider  a  dynamical  system  that  evolves  in  time  according  to  the  state  equations 

xn+i  =  Fnx„  -|-  wn,  (43) 

n  =  0, 1, 2, . .  where  {Fn}£L0  is  a  jpven  sequence  of  real  numbers  and  {u>n}£Lo  “  an 
independent,  identically  distributed  (i.i.d.)  sequence  of  zero  mean  random  variables.  The 
random  sequence  {tn„}  is  often  refered  to  as  a  white  noise  sequence  since  any  two  members 
of  the  sequence  are  uncorrelated,  i.e.,  F[u;ntcm]  =  0  for  n  ^  m.  The  noise-corrupted 
measurements  of  the  system  state  are  described  by  the  measurement  equation 


zn  —  Hnxn  +  vn,  (44) 

n  =  0,1,2,...,  where  Hn  is  a  proportionality  factor  (usually  a  constant)  which  is  built 
into  the  measuring  device.  In  general,  {Hn}%Lo  is  a  given  sequence  of  real  numbers.  The 
measurement  errors  or  measurement  noise  is  described  by  the  random  sequence  {t/n}  which 
is  a  zero  mean  i.i.d.  sequence.  It  is  further  assumed  that  the  sequences  {w„}  and  {v„}  are 
uncorrelated  and  that 


£[»$  =  (45) 

£[«$  =  <*£>  (46) 

EbnWm]  =  0,  (47) 

where  n,m  =  0, 1, 2, . . .,  and  where  a\  and  cr*  are  given  real  numbers.  (In  general,  the 
variances  of  vn  and  wn  may  depend  on  n,  and  the  results  of  this  section  are  easily  extended 
to  include  this  case.)  To  complete  the  specification  of  the  system  dynamics  it  is  necessary 
to  give  some  initial  condition  for  xq.  Instead  of  giving  a  fixed  initial  condition,  x<>  will  be  a 
random  variable  with  a  known  probability  density  function  /(x o).  Typically,  xo  is  normally 
distributed  with  mean  xo  and  variance  <Tq,  that  is 

X0~lV(xo,CTo). 

Furthermore,  the  random  variable  xo  is  assumed  to  be  uncorrelated  with  the  noise  sequences 
{«;„}  and  {«„}.  There  is  one  remark  which  should  be  made  about  the  initial  condition:  the 
PDF  f(x o)  is  not  needed  in  the  derivation  and  it  is,  therefore,  irrelevant;  it  is  only  necessary 
to  know  the  mean  xo  and  the  variance  of  the  random  variable  xo-  Now  to  the  problem 
at  hand.  In  simple  terms,  the  purpose  of  the  Kalman  filter  is  to  make  a  prediction  of  the 
future  state  xn+1  at  time  n  -f- 1  given  noise-corrupted  measurements  zo,  *i,  •  • .,  z„  from  the 
time  k  =  0  up  to  the  time  k  =  n. 

Problem  Statement:  Given  the  measurements  zo,  zi, . . .,  zn,  find  the  unbiased  least 
squares  estimate  xn+i  for  the  state  variable  x„+i.  Assume  that  the  estimate  has  the  linear 
recursive  form 

•^n+i  =  Knzn  +  Knxn.  (48) 


7 


Note  that  the  recursive  form  (48)  implies  that  the  estimate  xn+i  is  a  linear  combination 
of  the  random  variables  zo,  z\, . . . ,  zn.  Consequently,  xn+1  is  a  random  variable  too,  and  it 
makes  sense  to  discuss  the  expectation  of  zn+i-  This  will  appear  in  the  derivation. 

Problem  Solution:  If  the  estimate  xn+i  is  unbiased,  then  by  definition  E[xn+\  - 
zn+1]  =  0.  Using  equations  (43)  and  (48)  one  has 


®n+l  —  xn+l  —  (Kn^ri  KnXn)  {F nxn  "I"  ^n)  • 


(49) 


Substituting  equation  (44)  and  rearranging  terms  this  yields 


in+i  -  Zn+1  =  ( KnHn  -  Fn)xn  +  K'nxn  +  Knvn  -  wn.  (50) 


Taking  the  expectation  of  both  sides  of  this  equation  gives 

E[xn+1  -  xn+1]  =  (KnHn  -  Fn)E[xn]  +  K'nE[xn]  =  0.  (51) 

But  by  hypothesis  xn  is  unbiased,  and  therefore  E[xn]  =  E[in].  Thus,  equation  (51)  implies 

K'n  =  Fn~  KnHn.  (52) 


Having  solved  for  K'n,  it  is  now  necessary  to  determine  Kn  and  the  derivation  will  be 
complete. 

The  coefficient  Kn  must  be  chosen  to  minimize  the  mean  square  error 

£  =  £[(xn+i  -  xn+1)2].  (53) 

Using  the  result  (52),  equation  (50)  can  be  written 

(*n+l  *n+l)  ~  (Fn  Knffn)(xn  —  zn)  +  Knvn  —  wn.  (54) 

Squaring  this  equation  and  taking  the  expectation  of  both  sides  yields 

E[(xn+i  -  Zn+02]  =  (Fn  -  KnHn)2E[(xn  -  Zn)2]  +  Kiel  +  ol .  (55) 

It  is  useful  to  define  the  error  variance 


Pn  =  £[(z„-x„)2].  (56) 

The  previous  equation  may  then  be  written  in  the  form 

Fn+i  =  (Fn  -  KnHnfPn  +  K\ol  +  <r2 .  (57) 

This  equation  expresses  the  mean  square  error  £  =  Pn+i  as  a  function  of  Kn.  From  elemen¬ 
tary  calculus,  this  will  be  a  minimum  when 


8 


This  implies 


=  2 (F*  -  KnHn)(-Hn)Pn  +  2Kna\  =  0. 


(58) 


dPn+l 

dKn 

Solving  for  Kn,  one  finds 


Kn  = 


FnHnPn 


HlPn+oV 

To  verify  that  this  is  a  minimum  and  not  a  maximum  take  the  second  derivative 


(59) 


=  !HlP„  +  2<rl  >  0. 


Since  H%,  Pn,  and  cr*  are  aU  positive,  the  second  derivative  is  greater  than  zero  which 
implies  that  the  value  of  Kn  given  by  (59)  is  indeed  a  minimum.  Substituting  the  result 
(59)  into  equation  (57)  yields 


Pn+i  — 


n+1  H>Pn+ol 


+  o. 


tin 


which  may  be  written  in  the  equivalent  form 

Pn+l  =  Pn{Pn  KnHn)Pn  +  (T^. 


This  completes  the  derivation  in  the  1-dimensional  case. 

In  summary,  the  Kalman  filter  equations  in  1-dimension  are  given  by 


(60) 


(61) 


®n+ 1  —  FnXn  +  Fn{zn  ~  HnXn) , 


Pn+l  = 


Kn  = 


(F„  -  KnHn)FnPn  + 
FnHnPn 

PJK+oT 


(62) 

(63) 

(64) 


where  n  =  0,1,2, ....  In  an  actual  implementation  of  the  Kalman  filter,  the  input  is  the 
measurement  sequence  zn  and  the  output  is  the  sequence  of  state  predictions  xn+i.  In 
real-time  applications,  the  input  and  output  are  sequences  of  real  numbers.  The  output 
xn+i  is  obtained  from  the  input  zn  through  equations  (62),  (63),  and  (64).  In  order  to 
initialize  the  Kalman  filter  algorithm,  it  is  necessary  to  specify  the  initial  conditions.  The 
initial  conditions  are  given  by 


x0  =  xo,  (65) 

Po  =  <rl  (66) 

It  is  interesting  that  the  sequences  Pn  and  Kn  are  not  dependent  on  the  measurements. 
Therefore,  the  sequences  Pn  ^nd  Kn  can  be  precomputed  before  any  measurements  are 
taken.  Of  course,  these  quantities  can  also  be  computed  in  real-time  if  desired.  In  conclu¬ 
sion,  one  implementation  of  the  Kalman  filter  algorithm  is  as  follows: 

1.  Set  n  =  0, 
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2.  Initialize  *o  =  x0, 

3.  Initialize  Po  =  cr£, 

4.  Calculate  Kn  from  equation  (64), 

5.  Calculate  £n+i  from  (62), 

6-  Calculate  Pn+i  from  (63), 

7.  Increment  n  and  goto  step  4. 

5  Kalman  Filter  in  n- Dimensions 

In  n-dimensions,  the  system  is  characterized  by  an  n-dimensional  state  vector  x  which 
satisfies  a  state  equation  of  the  form 

x*+i  =  FjfcX*  +  wfc,  (67) 

where  A:  =  0, 1, 2, . .  {Ft}  is  a  given  sequence  ofnxn  real  matrices,  and  {w*}  is  an  i.i.d. 
sequence  of  zero  mean  random  vectors.  The  measurements  of  the  system  outputs  are  related 
to  the  state  vector  by  the  measurement  equation 

2k  =  HjfcX*  +  v*,  (68) 

where  zjt  is  an  m-dimensional  vector  of  measurements,  {H*}  is  a  given  sequence  ofmxn 
matrices,  and  {v*}  is  an  i.i.d.  sequence  of  zero  mean  random  vectors.  To  complete  the 
specification  of  the  sequences  {v*}  and  {w*},  it  is  necessary  to  know  the  covariance  matrices 

E[vkvfi  =  R*,  (69) 

Efatwin  =  Q/b  (70) 

where  k  =  0, 1,2, . . ..  In  addition,  the  sequences  {v*}  and  {w*}  are  uncorrelated  so  that 

£[vjw*]  =  0,  (71) 

where  j  and  k  are  any  non-negative  integers.  The  sequences  of  matrices  {R*}  and  {Q*}  are 
assumed  to  be  given.  In  addition,  these  are  both  symmetric  and,  by  assumption,  positive 
definite  matrices  for  each  k.  Finally,  the  initial  condition  on  the  state  equation  is  that  xo 
is  a  random  vector  with  a  probability  density  function  /(x o).  As  remarked  in  section  4, 
it  is  not  strictly  necessary  to  know  the  PDF  of  Xo,  but  only  the  mean  Xo  and  covariance 
matrix  P0  =  F[(xo  -  xo)  (xo  -  xo)T]  •  Usually,  x0  has  a  multivariate  normal  distribution  with 
known  mean  and  covariance  matrix.  Furthermore,  it  is  required  that  Xo  be  uncorrelated 
with  the  sequences  {v*}  and  {w*}.  Before  proceeding  with  the  derivation  of  the  Kalman 
filter  equations,  observe  that  the  vectors  x„  and  z„,  which  are  defined  by  equations  (67) 
and  (68),  are  both  random  vectors. 
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Problem  Statement:  Given  the  measurements  Zo,Zi,...,Zn,  find  the  unbiased  least 
squares  estimate  xn+i  for  the  state  vector  x„+i.  Assume  that  the  estimate  has  the  linear 
recursive  form 

Xn+1  =  KnZn  ■+•  KnXn.  (72) 

As  in  the  1-dimensional  case,  the  vector  xn+i  is  random  since  it  is  a  function  of  the  random 
vectors  Zo,  Zj, . . . ,  z„. 

Problem  Solution:  Since  the  estimate  x„+i  is  unbiased,  this  implies  F[xn+i  -  Xn+i]  = 


0.  Prom  equations  (67)  and  (72), 

xn+i  -  Xn+i  =  (K„zn  +  Kj,x„)  -  (Fnx„  +  wn).  (73) 

Substituting  equation  (68)  for  zn  and  rearranging  terms  this  becomes 

x„+i  -  xn+i  =  (KnH„  -  Fn)xn  +  K^Xn  +  K„vn  -  wn.  (74) 

Taking  the  expectation  of  both  sides  of  this  equation  gives 

£[Xn+i  -  X„+1]  =  (K„H„  -  F n)E[*n]  +  K^x*].  (75) 

But  by  hypothesis  xn  is  unbiased,  and  therefore  JS[x n]  =  £[xnj.  Thus,  equation  (75)  implies 

K;  =  Fn  -  K„H„.  (76) 


This  is  the  solution  for  K'n. 

Next,  the  coefficient  Kn  will  be  obtained  by  minimizing  the  mean  square  error.  This  is 
similar  to  the  procedure  used  in  the  1-dimensional  case.  The  mean  square  error  is  given  by 

£  =  ^[llXn+l  -  X^  ||2]  =  £[(5c„+l  ~  Xn+1)T(icn+1  -  X„+1)].  (77) 

For  the  purpose  of  taking  derivatives,  it  is  convenient  to  express  this  in  the  form  of  a  trace. 
Therefore,  write 

e  =  Tr  £[(Xn+l  -  Xn+lHXn+i  -  Xn+i)7'].  (78) 

Now  define  a  new  quantity 

Pn+1  =  £[(*»+!  -  Xn+l)(x„+l  -  Xn+l)1].  (79) 

This  is  called  the  error  covariance  matrix.  Then  the  mean  square  error  becomes 

e  =  Tr  (Pn+i).  (80) 


In  order  to  evaluate  this,  start  by  using  equations  (74)  and  (76)  to  write 

(xn+i  ~  xn+i)  =  (Fn  —  KnHn)(xn  —  x„)  +  (Knvn  —  wn).  (81) 
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Thus, 


(Xn+l  -  Xn+i)(5cn+1  -  Xn+i)T  =  [(F„  -  K„Hn)(x„  -x„)+  (K„vn  -  wn)] 

x  [(x„  -  Xn)T(Fj  -  HK)  +  KKl  -  wl)).  (82) 

By  expanding  the  product  and  then  using  the  linearity  of  the  expectation,  this  equation 
yields 

Pn+1  =  (F«  -  KnHn)P„(Fj  -  H£K£)  +  KnRnKl  +  Qn,  (83) 

where 


Pn  =  EKXn-XnKXn-Xn)1],  (84) 

R,  =  F[vnv£,  (85) 

Qn  =  E[ wnw£}.  (86) 

Of  course,  use  has  been  made  of  the  fact  that  Xn  and  x„  are  both  uncorrelated  with  vn  and 

wn.  By  expanding  the  first  term  in  equation  (83)  this  may  be  written 

Pn+1  =  FnPnF£  -  FnPnH£K£  -  KnHnPnFj[ 

+  KnHnPnH^Kj  +  KnR„K£  +  Qn.  (87) 

To  determine  Kn,  it  is  necessary  to  minimize  the  mean  square  error 

e  =  Tr  (Pn+i).  (88) 


Note  that  the  trace  is  a  scalar  quantity  and  therefore  e  is  a  scalar  function  of  the  elements 
(Kn)ij  of  the  matrix  K„.  Furthermore,  since  Pn  is  independent  of  K„,  the  minimum  can 
be  obtained  by  straightforward  differentiation.  Thus, 

st-=0'  (89> 


for  t  =  1, . . .,  m,  and  j  =  This  may  be  written  more  compactly  in  the  matrix  form 


de 

8K 


=  0, 


(90) 


where  the  indicated  derivative  is  a  matrix  with  the  i,j  element  given  by  equation  (89).  The 
derivative  can  be  computed  easily  with  the  use  of  the  following  identities: 


(AS)  =  a-. 

(91) 

!BAT)  =  B 

(92) 

^-Tr  (ABAr)  =  2AB. 

(93) 
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These  are  easily  proved.  (For  example,  write  out  the  matrix  products  for  the  case  when  all 
the  matrices  are  2  x  2  and  then  take  the  derivative  of  the  trace.)  Using  these  formulas  to 
differentiate  the  trace  of  equation  (87)  it  is  a  simple  matter  to  obtain 

=  -F„PnF£  -  (HnPnF£)T  +  2KnH„P„H£  +  2KnRn  =  0.  (94) 

c/Kn 

Rearranging  terms,  this  becomes 

Kn(HnPnH*  +  Rn)  =  F„PnH£.  (95) 

Note  that  since  both  Pn  and  RL„  are  symmetric  matrices,  the  term 

(HnPnEtf+Rn)  (96) 

is  also  symmetric.  Furthermore,  since  both  Pn  and  Rn  are  positive  definite,  this  implies 
that  the  term  (96)  is  also  positive  definite.  Consequently,  all  its  eigenvalues  are  positive 
which  implies  that  it  is  nonsingular.  Therefore,  the  inverse  exists  and  the  solution  of  (95) 
is  given  by 

Kn  =  F„PnH£(HnPnH£  +  Rn)'1.  (97) 

This  is  the  maun  result.  To  be  rigorous,  it  must  be  verified  that  this  is  indeed  a  minimum 
and  not  a  maximum.  This  detail  is  omitted  for  the  sake  of  brevity. 

To  complete  the  derivation  the  result  (97)  for  Kn  may  be  substituted  back  into  equation 
(87)  to  find  a  simple  form  for  Pn+i-  Substituting  (97)  into  (87)  gives  Pn+i  = 

F„PnF£  -  FnP„Hj[(HnPnH£  +  Rn)-1]TH„PnFj 
-  FnP„H^(H„PnH^  -f-  Rn)~ 1HnP„Fn 

+  FnP  nHj  (HnP  nHj  +  R„)-1HnPnH^[(HnPnH^  +  Rn)""1]TH„P  „Fj 
+  F„PnHj(H„PnHn  +  Rn)-1Rn[(HnP„H£  +  Rw)“1]THnPnF; 

+  Qn.  (98) 

Since  the  term  (96)  is  symmetric,  the  second  to  last  two  terms  in  (98)  can  be  combined  to 
give 

Pn+l  =  F„PnF£  -  F„PnH£(HnPnH£  +  Kn^I^F*  +  Qn.  (99) 

This  may  be  further  simplified  using  (97).  Hence, 

Pn+i  =  (F„  -  KnHrt)P„Fj  +  Q„.  (100) 

This  is  the  desired  result. 

This  completes  the  derivation  of  the  Kalman  filter  in  n-dimensions.  In  summary,  the 
Kalman  filter  equations  are  given  by 

^n+l  =  F nXn  "I”  K„(zn  —  Hnx„), 

Pn+1  =  (F„-KnHn)PnF£+Qn, 

Kn  =  FnPnH^HnPnH^  +  Rn)-1, 
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(101) 

(102) 

(103) 


where  n  =  0, 1, 2, . .  -  The  initial  conditions  are  given  by 

xo  =  5to, 

Po  =  Po- 

One  implementation  of  the  Kalman  filter  algorithm  is  as  follows: 

1.  Set  n  =  0, 

2.  Initialize  Jto  =  *o, 

3.  Initialize  Po  =  Po, 

4.  Calculate  Kn  from  equation  (103), 

5.  Calculate  5t„+i  from  (101), 

6.  Calculate  Pn+j  from  (102), 

7.  Increment  n  and  goto  step  4. 

If  desired,  the  sequences  Kn  and  Pn  may  be  precomputed  before  running  the  algorithm. 

6  Standard  Kalman  Filter  Equations 

In  some  instances,  it  is  necessary  to  estimate  the  current  state  x„  based  on  measurements 
up  to  and  including  zn.  In  the  last  section,  the  measurements  zo,*i, . . were  used  to 
estimate  the  state  Xn+1.  In  this  section,  the  measurements  zo,Zi, . .  .,Zn  will  be  used  to 
estimate  the  state  Xr.  Assuming  a  recursive  form  of  solution,  the  calculation  proceeds  in 
exactly  the  same  way  as  in  the  previous  section.  However,  it  must  be  understood  that  the 
quantities  xn,  Pn,  and  K„  which  appear  in  this  section  are  not  the  same  as  those  in  section 
5.  Relationships  between  the  two  solutions  will  be  obtained  in  section  8.  For  the  time 
being,  the  results  of  this  section  are  completely  independent  of  those  of  the  last  section.  A 
separate  solution  for  the  1-dimensional  problem  is  omitted  since  it  is  a  special  case  of  the 
more  general  n— dimensional  solution.  The  interested  reader  may  derive  the  solution  to  the 
1-dimensional  case  as  an  excersize. 

A  word  about  terminology.  The  term  “standard  Kalman  filter”  is  not  a  part  of  the 
Kalman  filtering  literature  nor  is  it  commonly  used  by  workers  in  this  field.  It  has  been 
chosen  by  the  author  because  the  recursive  solution  to  the  linear  prediction  problem  of  all 
orders  may  be  obtained  directly  from  the  solution  of  the  “standard”  Kalman  filter  equa¬ 
tions.  In  section  8,  for  example,  it  will  be  shown  how  the  1-step  prediction  may  be  obtained 
from  the  standard  solution. 


(104) 

(105) 


14 


Problem  Statement:  A  linear  system  obeys  the  state  space  equations  (67)  together 
with  the  measurement  equation  (68).  Given  the  measurements  *o,*i, . .  .,*n>  find  the  un¬ 
biased  least  squares  estimate  Xn  of  the  state  x„.  Assume  that  the  estimate  has  the  linear 
recursive  form 

Xn  =  K„Zn  +  K&-1*  (106) 

where  n  =  1, 2,  — 

Problem  Solution:  To  begin,  use  (106)  to  write 

*n  -  Xn  =  (Kn*n  +  K^X„_i)  -  X„.  (107) 

Substituting  for  Xn  and  zn  from  the  state  equation  (67)  and  the  measurement  equation 
(68),  this  becomes 

xn  —  Xn  —  (KnHn  1)F  n_iXn_i  +  Knin_i  +  (KnHn  —  l)wn— 1  +  Knvn.  (108) 

Taking  the  expectation  of  this  equation,  the  requirement  that  the  estimate  be  unbiased 
implies 

£[Xn  -  Xn]  =  (K„H„  -  lJFn.^fXn-x]  +  K;F[x„-l]  =  0.  (109) 

Therefore,  since  £[5Cn_i  -  xn-i]  =  0,  this  yields 

K;  =  (l-KnHn)F„_,.  (110) 

Substituting  this  back  into  equation  (108),  it  follows  that 

(x„  -  Xn)  =  (1  -  KnHn)Fn_i(Xn_i  -  Xn— i)  +  Knvn  +  (KnHn  -  l)w„_i.  (Ill) 
Next,  the  factor  K„  is  determined  by  minimizing  the  mean  square  error 

£  =  ||Xn  -Xn||2.  (112) 

To  simplify  the  process  of  taking  derivatives,  write  this  in  the  form  of  a  trace,  that  is, 

e  =  Tr  ^[(Xn  -  Xn)(x„  -  Xn)7!-  (113) 

Using  equation  (111)  together  with  the  linearity  of  the  expectation,  this  yields 

£[(Xn  -  Xn)(xn  -  Xn)7]  = 

(1  -  KnHn)Fn— iF[(xn_i  -  xn_i)(xn_i  -  xn— i)7]Fj_|(l  -  K„Hn)r 

+  K„£[v„v£]K£  +  (K„H„  -  l)-^[w„_ |wJ_1](KnHn  -  1)T.  (114) 

Defining  the  quantity  Pn  by 

Pn  =  £[(*„  -  x„)(x„  -  Xn)7],  (115) 
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the  last  equation  becomes 


P„  =  (l-K„Hn)F„-,Pn-1F;.1(l-KnH„)’- 

+  K„R.KJ  +  (1  -  K.Hn)Q„_,  (1  -  KnHn)T.  (116) 

Note  that  Pn-i  is  independent  of  Kn,  and  therefore  is  a  constant  for  purposes  of  differen¬ 
tiation.  Now,  the  mean  square  error  is  given  by 

e  =  Tr  (Pn),  (117) 

and  this  will  be  a  minimum  if 

^-Tr  (Pn)  =  0,  (118) 

where  the  derivative  with  respect  to  a  matrix  is  defined  componentwise  as  in  (89).  Expand¬ 
ing  the  product  in  equation  (116)  yields 

Pn  =  Fn_1Pn_1Fj_1-F„_iPn_1Fj_1HK-KnHnF„-iPn_1Fj_1 
+  KnHnFn_1Pn_1F];_1HjKj[  +  KnRnK*  +  Qn_x 

-  Q„-iHK  -  KnHnQ„_i  +  KnHnQn.xHK.  (119) 

Taking  the  trace  of  this  equation  and  differentiating  using  the  identities  (91),  (92),  and  (93), 
it  follows  that 

^-IV(Pn)  =  — 2Ffl_iPn_1Fj_iH£  +  2KnH„Fn_1Pn_iF£_1H£ 

+  2KnRn  -  2Qn_xB£  +  2K„HnQ„_1H£.  (120) 

The  fact  that  Rn  and  Q„  are  both  symmetric  has  also  been  used.  Equating  this  to  zero 
and  collecting  terms  yields 

Kn  [H„F„_,P„_1Fj_1Hj  +  Rn  +  HnQn^H"  = 

[F„-,Pn-lFj_1Hj  +  Qn-iHJ] .  (121) 

This  expression  can  be  made  more  manageable  by  making  the  definition 

Mn  =  FnP„F£  +  Q„.  (122) 

Hence,  equation  (121)  may  be  written 

Kn  [HnMn_iHj  +  Rn]  =  (123) 

If  the  inverse  exists,  then  the  solution  for  K„  takes  the  form 

Kn  =  Mn.,H;  [HnM„_iH£  +  Rn]'1 .  (124) 

But  it  is  easy  to  see  that  the  inverse  does  exist  since  R„,  Q„,  and  Pn,  are  all  symmetric, 
positive  definite  matrices,  and  all  positive  definite  matrices  are  nonsingular.  Therefore, 
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(124)  is  the  final  solution  for  Kn.  The  proof  that  (124)  is  indeed  a  minimum  and  not  a 
maximum  will  be  omitted. 

The  expression  (119)  for  Pn  can  be  simplified.  First,  write  equation  (119)  in  the  form 

Pn  =  Kn(HnM„_iH^  +  R«)K^ 

-  K„HnMn_i  -  +  M„_i.  (125) 

Substituting  the  result  (124)  for  Kn  in  the  first  term,  but  not  for  K£,  the  first  and  third 
terms  cancel,  leaving  the  result 


Pn  =  (l-KnHn)Mn_!.  (126) 

This  is  the  desired  form. 

In  summary,  the  standard  Kalman  filter  equations  are  given  by 

kn  =  Kn*»  +  (l-KnHn)F„_1xn-i,  (127) 

Kn  =  Mn-iHj  [HnMn.iHj  +  Rn]"1  ,  (128) 

Mn-1  =  Fn-lPn-lFj_i  +  Qn-1>  (129) 

Pn  =  (l-KnHn)Mn-i,  (130) 


where  n  =  1,2,..-  In  order  to  initialize  the  Kalman  filter  algorithm,  it  is  necessary  to 
define  initial  conditions  for  xo,  Ko,  and  Po-  These  initial  conditions  will  be  derived  in  the 
next  section. 

7  Initial  Conditions  for  the  Standard  Kalman  Filter 

To  determine  the  initial  conditions,  the  problem  is  to  find  the  estimate  5b)  of  the  initial 
state  xo  based  on  the  one  measurement  Zo-  The  solution  to  this  problem  is  straightforward 
and  may  be  obtained  by  the  methods  used  previously.  Once  the  initial  conditions  are  de¬ 
termined,  a  summary  of  the  resulting  Kalman  filter  algorithm  will  be  presented. 

Problem  Statement:  Let 

z  =  Hx  +  v,  (131) 

where  x  is  a  random  vector  with  mean  X  and  covariance  matrix  P,  and  v  is  a  zero  mean 
random  vector  which  is  uncorrelated  with  x.  Find  the  unbiased,  linear,  least  squares  es¬ 
timate  x  of  the  vector  x  based  on  the  measurement  z.  Assume  that  the  estimate  has  the 
form 

x  =  Kz  -I-  K'X.  (132) 

This  problem  bears  a  close  resemblance  to  the  least  squares  problem  of  section  2.  The 
two  problems,  however,  are  fundamentally  different.  The  difference  is  that  in  equation  (8), 
section  2,  the  vector  x  was  a  constant,  whereas  in  equation  (131)  the  vector  x  is  a  random 
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variable. 


Problem  Solution:  To  begin,  use  equations  (131)  and  (132)  to  write 

(x  -  x)  =  (KH  -  l)x  +  K'X  +  Kv.  (133) 

Taking  the  expectation  and  using  the  requirement  that  x  is  unbiased,  it  follows  that 

K'  =  (1  -  KH).  (134) 

bstituting  this  back  into  (133)  yields 

(x  -  x)  =  (KH  -  1)  (x  -  X)  +  Kv.  (135) 

To  determine  K  it  is  necessary  to  minimize  the  mean  square  error 

£  =  TrE[(x-x)(X-xH.  (136) 

Using  equation  (135)  together  with  the  linearity  of  the  expectation,  it  follows  that 

E[(x  -  x)(X  -  x)7]  =  (KH  -  l)E[(x  -  X)(x  -  Xj^KH  -  l)r 

+  KRKT.  (137) 

Making  the  definition 

P  =  £[(x  -  x)(x  -  x)7],  (138) 

and  using  the  given  covariance  matrix 

P  =  E[(x  -  X)(x  -  X)7],  (139) 

this  equation  becomes 

P  =  (KH  -  1)P(KH  -  l)7  +  KRK7.  (140) 

Then,  expanding  the  product,  this  may  be  written 

P  =  KHPHrK7  -  KHP  -  PHrK7  +  P  +  KRK7.  (141) 

Taking  the  trace  of  this  equation  and  then  the  derivative  with  respect  to  K  yields 

^  =  2KHPHt  -  2PHt  +  2KR  =  0.  (142) 

And,  finally,  solving  this  equation  for  K, 

K  =  PHt(HPHt  +  R)~1.  (143) 

This  is  a  familiar  result.  Clearly,  the  inverse  in  (143)  exists  since  P  and  R  are  both  positive 
definite. 
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To  find  a  simpified  form  for  P,  first  write  equation  (140)  in  the  form 

P  =  K(HPHt  +  R)Kr  -  KHP  -  PHTKr  +  P.  (144) 

Then,  substituting  (143)  for  K  in  the  first  term  but  not  for  KT,  the  first  and  third  terms 
cancel,  leaving 

P  =  (1  -  KH)P.  (145) 

This  is  the  desired  form. 

Having  solved  the  above  estimation  problem,  the  initial  conditions  for  the  standard 
Kalman  filter  equations  may  be  obtained  by  simply  putting  a  subscript  ‘0’  on  each  of  the 
equations  (132),  (143),  and  (145).  Thus, 


Xo  =  Xo  +  K0(zo  —  HoXo), 

(146) 

Po  =  (1  -  KoHo)Po» 

(147) 

Ko  =  PoH^HoPoHS  +  Ro)-1. 

(148) 

An  outline  of  the  complete  Kalman  filter  algorithm  is  as  follows: 

1.  Initialize  Ko  using  (148), 

2.  Initialize  xo  using  (146), 

3.  Initialize  P0  using  (147), 

4.  Set  n  =  1, 

5.  Calculate  M„_i  from  (129), 

6.  Calculate  Kn  from  equation  (128), 

7.  Calculate  the  state  estimate  x„  from  (127), 

8.  Calculate  Pn  from  (130), 

9.  Increment  n  and  goto  step  5. 

8  Prediction-Correction  Formulation 

In  this  section,  a  relation  will  be  found  between  the  state  estimate  Xn  of  section  6,  and 
the  state  prediction  x„+i  of  section  5.  Using  this  result,  the  preveously  separate  results  of 
sections  5  and  6  may  be  combined  into  one  unified  set  of  equations  for  both  the  current  state 
estimate  xn  (section  6)  and  the  state  prediction  x„  (section  5).  The  unified  set  of  Kalman 
filter  equations  consists  of  the  standard  Kalman  filter  equations  of  section  6,  together  with 
a  new  equation  for  the  state  prediction  xn+i  in  terms  of  the  current  state  estimate  xn.  This 
new  equation  will  be  derived  in  this  section.  First,  to  prevent  confusion,  it  is  necessary  to 
introduce  some  new  notation. 
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Let  x(»|n)  be  the  estimate  of  the  state  xn  based  on  the  measurements  Zo, . .  .,z„,  and 
let  x(n+l|n)  be  the  estimate  of  the  state  x„+1  based  on  the  measurements  zo, . . .,zn-  Then 
x(njn)  is  the  solution  to  the  Kalman  filter  equations  (127),  (128),  (129),  and  (130),  which 
were  derived  in  section  6.  And  x(n+l|n)  is  the  solution  to  the  Kalman  predictor  equations 
(103),  (104),  and  (105),  which  were  derived  in  section  5.  It  will  now  be  shown  that  the 
one-step  prediction  x(n-(-l|n)  may  be  obtained  from  the  current  state  estimate  x(n|n)  by 
the  simple  relation 

x(n+ljn)  =  Fnx(n|n).  (149) 

A  simple,  heuristic  proof  will  be  given  here.  A  more  detailed  proof  is  given  in  the 
appendix.  Begin  with  the  fact  that,  by  definition,  the  estimate  x(n|n)  and  the  estimate 
x(n-f  l|n)  are  both  linear  combinations  of  the  measurements  zo,zi, . .  .,zn.  Therefore,  it  is 
reasonable  to  assume  that  they  are  related  by  a  linear  transformation  of  the  form 


x(n+l|n)  =  A„x(n|n),  (150) 

where  A„  is  a  square  matrix.  From  the  state  equation  (67)  it  follows  that 

x(n+l|n)  -  Xn+i  =  A„x(n|n)  -  (Fnx„  +  wn).  (151) 

Taking  the  expectation  of  this  equation  yields 

£'[x(n+l|n)  -  Xn+i]  =  A«£[:ft(n|n)]  -  Fn£[xn]  =  0.  (152) 

But,  since  x(n|n)  is  an  unbiased  estimate  of  Xn, 

J?[x(n|n)]  =  F[x„].  (153) 

Therefore,  equation  (152)  implies 

An  =  F„.  (154) 

This  is  the  desired  result. 

By  adding  equation  (149)  to  the  standard  Kalman  filter  equations  (127)-(130),  these 
may  be  rewritten  in  the  following  form: 

x(n|n)  =  x(n+l|n)  +  K„[z„-H„x(n|n-l)],  (155) 

x(n+l|n)  =  F„x(njn),  (156) 

K„  =  Mn_iH£[HnMn_iH£  +  Rn]~\  (157) 

1  =  Fn-lPn— lFn_i  -hQn— 1,  (1^8) 

P„  =  (l-KnHnJM,,.!,  (159) 

where  n  =  0, 1, 2,  —  The  initial  conditions  are  now  given  by 

x(0|  -  1)  =  xo,  (160) 

=  P0.  (161) 


When  written  in  this  way,  the  Kalman  filter  is  a  combination  of  the  state  update  equa¬ 
tion  (155)  and  the  state  prediction  equation  (156).  These  are  commonly  refered  to  as  the 
prediction-correction  formulas.  This  is  a  convenient  form  for  most  applications.  A  direct 
implementation  of  the  Kalman  filter  algorithm  is  as  follows: 
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1.  Initialize  x(0|  -  1)  =  Xo, 

2.  Initialize  M_i  =  P0, 

3.  Set  n  =  0, 

4.  Calculate  Kn  from  (157), 

5.  Calculate  x(n|n)  from  (155), 

6.  Calculate  x(n+ljn)  from  (156), 

7.  Calculate  Mn_i  from  (158), 

8.  Calculate  Pn  from  (159), 

9.  Increment  n  and  goto  step  4. 

Having  presented  the  mathematical  theory  of  the  Kalman  filter  algorithms,  it  is  impor¬ 
tant  to  note  that  there  are  certain  pitfalls  involved  with  the  computational  aspects  of  these 
algorithms.  These  problems  are  inevitable  due  to  the  finite  precision  of  digital  computers. 
More  information  about  the  practical  implementation  of  the  Kalman  filter  can  be  found  in 
the  literature.  [14]-[16] 

9  Summary 

A  rigorous  derivation  of  the  Kalman  filter  equations  has  been  presented  which  is  ap¬ 
propriate  for  an  introductory  short  course  on  Kalman  filter  theory.  The  way  it  has  been 
defined,  the  Kalman  filter  gives  the  optimum  unbiased,  linear,  least  squares  estimate  of 
the  state  vector  based  on  all  past  and  current  measurements.  At  this  point,  it  is  strongly 
recommended  that  the  reader  study  a  few  applications  of  the  Kalman  filter  to  see  how  the 
filter  is  used  in  practical  problems.  One  application  is  in  the  radar  tracking  of  airborne  tar¬ 
gets.  This  application  is  well  described  in  some  of  the  original  papers.[17]-[19]  The  range 
of  applications  of  the  Kalman  filter  is  vast  and  varied.  More  applications  can  be  found  in 
the  IEEE  reprint  series  Kalman  Filtering:  Theory  and  Application.[l4] 
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Appendix 

Proof  of  Equation  149 
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The  purpose  of  this  appendix  is  to  give  an  alternate  proof  of  equation  (149)  in  section 
8.  It  is  easiest  to  prove  this  in  the  1-dimensional  case  first.  By  the  definition  of  x(n+l|n), 


E[j|i(n+l|n)-x„+i||2]  =  min.  (1) 

In  this  notation,  the  minimum  is  taken  with  respect  to  ail  linear  combinations  x(n+ l|n)  of 
the  measurements  zq, . . . ,  z*.  Alternatively,  consider 

F[||F„x(»|n)  -  xn+1  H2]  =  F[||F„x(n|n)  -  Fnxn  -  wnf]  (2) 

=  F[||F„x(n|n)  -  Fnxn||2]  +  E[\\wn\\%  (3) 

where  the  last  inequality  follows  since  both  xn  and  x(n|n)  are  uncorrelated  with  wn.  Using 
the  linearity  of  the  expectation  this  becomes 

£[||F„i(»|n)  -  *n+1||2l  =  Fn!B[||i(r.|n)  - 1„||2]  +  EUKII2].  (4) 

But,  by  the  definition  of  x(njn), 

F[||x(n|n)  -  Xnll2]  =  min.  (5) 

Consequently,  equation  (4)  implies 

F[||F„x(n|n)  -  xn+i  ||]  =  min.  (6) 


Now,  recall  that  x(n|n)  and  x(n-fl|n)  are  both  linear  combinations  of  the  measurements 
zq,  zi,  . . . ,  zn.  Therefore,  if  the  linear  combination  that  minimizes  (1)  is  unique,  then  equar 
tions  (1)  and  (6)  together  imply 


x(n+l)n)  =  F„x(n|n). 


(7) 


This  completes  the  proof  in  the  1-dimensional  case. 

The  proof  in  the  n— dimensional  case  is  similar.  By  the  definition  of  the  estimate  x(n-|- 

H»), 

F[||x(n+l|n)  -  Xn+ijj2]  =  min.  (8) 

On  the  other  hand, 

F[||Fnx(n|n)  -  x„+i||2]  =  F[||F„x(n|n)  -  Fnx„  -  wn||2]  (9) 

=  F[||F„x(n|n)  -  Fnxn||2]  +  F[||wn||2],  (10) 


where  the  second  equality  follows  from  the  fact  that  both  xn  and  x(n|n)  are  uncorrelated 
with  wn.  Now,  if  the  first  term  on  the  right-hand-side  is  a  minimum,  that  is,  if 

F[||Fnx(n|n)  -  F„xnj|2]  =  min,  (11) 

then  this  implies 

F[||Fnx(n|n)  -  xn+i||2]  =  min.  (12) 
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Now,  observe  that  both  F„x(n|n)  and  x(n+l|n)  are  linear  combinations  (really  multilinear 
combinations)  of  the  measurements  zo,  Zi, . . . ,  zn,  that  is, 


x(n+l|n)  =  Aozo  +  A1Z1  +  . . .  +  An*n,  (13) 

F„x(n|n)  =  BoZo  +  BiZ!  +  ...  +  Bnzn  (14) 

Therefore,  if  the  linear  combination  (13)  which  minimizes  the  mean  square  error  (8)  is 
unique,  then  equations  (8)  and  (12)  will  together  imply 

x(n+l|n)  =  F„x(n|n).  (15) 

This  is  the  desired  result.  Thus,  it  remains  to  prove  (11).  First,  note  that  this  may  be 
written 

£[||F„*(n|n)  -  =  FKFjF.uJ,  (16) 

where 

u„  =  x(n|n)  -  Xn.  (17) 

Setting 

A  =  F£Fn,  (18) 

then  clearly  A  is  symmetric,  and  if  Fn  is  nonsingular,  then  A  is  positive  definite.  To  prove 
the  desired  result,  it  will  be  shown  that 

£[<Aun]  =  min,  (19) 

if 

2?[uTu]  =  min,  (20) 

where  A  is  any  symmetric  positive  definite  matrix.  Since  A  is  symmetric,  it  is  orthogonally 
diagonalizable.  Thus,  there  exists  a  matrix  S  such  that 

SrAS  =  D,  (21) 

where  D  is  the  diagonal  matrix 


(22) 


and  Aj, . . .,  An,  are  the  eigenvalues  of  A.  Since  A  is  positive  definite,  all  its  eigenvalues  are 
positive  real  numbers.  Now,  one  may  write 

<Aun  =  ur(SDSr)u 
=  vrDv 

=  AxUx  +  . . .+  A„t’^, 
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(23) 

(24) 

(25) 


where 


V  =  STu. 

Taking  the  expectation  of  this  equation  yields 

(26) 

£[<Aun]  =  AxE[t>?]  +  . . .  +  AnE^S- 

Since  all  the  eigenvelues  are  positive,  this  is  a  minimum  if 

(27) 

E[i>x]  =  min, 

(28) 

E[v%\  =  min. 

Equivalently,  this  is  a  minimum  if 

(29) 

E[vtv]  =  min. 

But,  using  equation  (26), 

(30) 

vTv  =  uTSSTu  =  uTu. 

Hence,  the  condition  (26)  reduces  to 

(31) 

E[uTu]  =  min. 

(32) 

This  is  the  desired  result.  End  of  proof. 
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